//-*- C++ -*-
// Copyright 2008 Isis Innovation Limited
// 
// This header declares the Tracker class.
// The Tracker is one of main components of the system,
// and is responsible for determining the pose of a camera
// from a video feed. It uses the Map to track, and communicates 
// with the MapMaker (which runs in a different thread)
// to help construct this map.
//
// Initially there is no map, so the Tracker also has a mode to 
// do simple patch tracking across a stereo pair. This is handled 
// by the TrackTrails() method and associated sub-methods. 
// Once there is a map, TrackMap() is used.
//
// Externally, the tracker should be used by calling TrackFrame()
// with every new input video frame. This then calls either 
// TrackTrails() or TrackMap() as appropriate.
//

#ifndef __TRACKER_H
#define __TRACKER_H

#include "MapMaker.h"
#include "Scarf.h"
#include "ATANCamera.h"
#include "Relocaliser.h"
#include "PointSet.h"
#include "DynamicObject.h"
#include "SE3GaussianPDF.h"
#include "RBF.h"
#include "PolygonalMesh.h"
#include "SmallBlurryImage.h"
#include "external/ARToolKit/ARToolKitPlus/TrackerMultiMarker.h"

#include <sstream>
#include <vector>

class TrackerData;
class MarkerData;

struct DynObjInfo{
  bool init;
  int index;
  cv::Mat OldFrameMat;
  std::vector<cv::Point2f> OldFrameFeaturePos;
  boost::shared_ptr<KeyFrame> CurrentFrame;
};

class Tracker
{
public:
  Tracker(const ATANCamera &c, Scarf &s, MapMaker &mm);
  ~Tracker();
  
  // TrackFrame is the main working part of the tracker: call this every frame.
  bool TrackFrame(uint64_t timestamp, CVD::Image<CVD::byte> &greyFrame, CVD::Image<CVD::Rgb<CVD::byte> > &colourFrame, bool bDraw, SE3GaussianPDF &se3PdfGfromC, std::vector<TooN::Vector<3> > &vTest);

  //Funcion temporal para las pruebas "no reales"
 // void DrawOpticalFlow(CVD::Image<CVD::byte> &Image,SE3GaussianPDF &se3PdfGfromC, std::vector<TooN::Vector<3> > &vTest);
  DynObjInfo D;

  inline SE3<> GetCamFromWorld() { return mse3CamFromWorld; };

  // Gets messages to be printed on-screen for the user.
  std::string GetMessageForUser();

  void Reset(bool bCompleteReset = true);                   // Restart from scratch. Also tells the mapmaker to reset itself.

protected:
  boost::shared_ptr<KeyFrame> mCurrentKF;            // The current working frame as a keyframe struct
  
  // The major components to which the tracker needs access:
  MapMaker &mMapMaker;            // The class which maintains the map
  ATANCamera mCamera;             // Projection model
  ATANCamera mCameraSBI;             // Projection model
  Scarf &mScarf;          // Keypoint detector and descriptor
  Relocaliser *mpRelocaliser;       // Relocalisation module
  DynamicObject mDynamicObject;

  void RenderMesh();              // Draws the surface mesh
  void RenderGrid();              // Draws the reference grid

  // The following members are used for initial map tracking (to get the first stereo pair and correspondences):
  double mdInitBaseline;
  bool TrackMarker(uint64_t timestamp, CVD::Image<CVD::byte> &greyFrame, CVD::Image<CVD::Rgb<CVD::byte> > &colourFrame);
  double MaxBaseline(std::vector<boost::shared_ptr<KeyFrame> > &vKeyFrames);
  double ClosestKeyFrame(boost::shared_ptr<KeyFrame> kf, std::vector<boost::shared_ptr<KeyFrame> > &vKeyFrames);
  std::vector<boost::shared_ptr<KeyFrame> > mvInitialKeyFrames;
  std::map<int, MarkerMeasurements> mmMarkerMeasurements;
  
  // Methods for tracking the map once it has been made:
  void TrackMap(Map *pMap);                // Called by TrackFrame if there is a map.
  void AssessTrackingQuality();   // Heuristics to choose between good, poor, bad.
  void ApplyMotionModel(uint64_t timestamp);        // Decaying velocity motion model applied prior to TrackMap
  void UpdateMotionModel();       // Motion model is updated after TrackMap
  int SearchForPoints(std::vector<boost::shared_ptr<TrackerData> > &vTD, int nRange, int nFineIts);  // Finds points in the image
  Vector<6> CalcPoseUpdate(std::vector<boost::shared_ptr<TrackerData> > vTD, 
			   double dOverrideSigma = 0.0, 
			   bool bMarkOutliers = false); // Updates pose from found points.
  Vector<6> CalcPoseUpdate(std::vector<boost::shared_ptr<MarkerData> > vMD, double dOverrideSigma);
  SE3<> mse3CamFromWorld;           // Camera pose: this is what the tracker updates every frame.
  SE3<> mse3StartPos;               // What the camera pose was at the start of the frame.
  uint64_t mStartTime, mCamFromWorldTime;
  Vector<6> mv6CameraVelocity;    // Motion model
  double mdVelocityMagnitude;     // Used to decide on coarse tracking 
  double mdMSDScaledVelocityMagnitude; // Velocity magnitude scaled by relative scene depth.
  bool mbDidCoarse;               // Did tracking use the coarse tracking stage?
  
  bool mbDraw;                    // Should the tracker draw anything to OpenGL?
  int mnShowScale;

  // Interface with map maker:
  int mnFrame;                    // Frames processed since last reset
  int mnLastKeyFrameDropped;      // Counter of last keyframe inserted.
  int mnThisFramePoints;          // Number of points tracked in the most recent keyframe
  void AddCurrentKeyFrameToMap(CVD::Image<CVD::Rgb<CVD::byte> > &colour);          // Gives the current frame to the mapmaker to use as a keyframe
  void DeleteCurrentKeyFrame();       // Deletes the current frame as it is no longer required
  
  // Tracking quality control:
  int manMeasAttempted[LEVELS];
  int manMeasFound[LEVELS];
  enum {BAD, DODGY, GOOD} mTrackingQuality;
  int mnLostFrames;
  
  // Relocalisation functions:
  bool AttemptRecovery(CVD::Image<CVD::Rgb<CVD::byte> > &colour);         // Called by TrackFrame if tracking is lost.
  bool mbJustRecoveredSoUseCoarse;// Always use coarse tracking after recovery!

  // Frame-to-frame motion init:
  SmallBlurryImage *mpSBILastFrame;
  SmallBlurryImage *mpSBIThisFrame;
  void CalcSBIRotation();
  Vector<6> mv6SBIRot;
  
  // User interaction for initial tracking:
  bool mbUserPressedSpacebar;
  std::ostringstream mMessageForUser;

  int nLevelThreshFAST[4];
  int mnGlobalSE3;      // Should we use provided external position estimate (absolute measurements from GPS)
  int mnGlobalSO3;      // Should we use provided external attitude estimate (absolute measurements from compass, gyros etc.)
  int mnMinTrackedPoints;
  int mnDrawFASTCorners;
  double mdMaxRotationError;
  double mdMaxTranslationError;
  double mdMinInitBaseline;

  GVars3::gvar3<int> mgvnUseRelocaliserSBI;
  GVars3::gvar3<int> mgvnLevel1ThreshFAST;
  GVars3::gvar3<int> mgvnLevel2ThreshFAST;
  GVars3::gvar3<int> mgvnLevel3ThreshFAST;
  GVars3::gvar3<int> mgvnLevel4ThreshFAST;
  GVars3::gvar3<int> mgvnMinTrackedPoints;
  GVars3::gvar3<int> mgvnGlobalSE3;
  GVars3::gvar3<int> mgvnGlobalSO3;
  GVars3::gvar3<double> mgvdMaxRotationError;
  GVars3::gvar3<double> mgvdMaxTranslationError;
  GVars3::gvar3<int> mgvnDrawFASTCorners;
  GVars3::gvar3<double> mgvdMinInitBaseline;

  // GUI interface:
  void GUICommandHandler(std::string sCommand, std::string sParams);
  static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams);
  struct Command {std::string sCommand; std::string sParams; };
  std::vector<Command> mvQueuedCommands;

  ARToolKitPlus::TrackerMultiMarker *mpMultiMarkerTracker;
};

#endif

